%% 牛顿迭代计算制导
% 本例为空对地导弹
% 控制加速度a只改变速度方向，不改变速度大小
% 考虑空气阻力，从而影响飞行速度大小
% 控制量u: a
clear all;
clc;

%% 参数初始化
position = [0; 2500];   %飞行器初始位置(m)
velocity = 300;         %飞行器初始速度(m/s)
fpAngle = 0;            %航迹角(飞行器速度与水平面夹角)(rad)
posDesired = [5000; 0]; %飞行器末端期望位置(m)
fpaDesired = -pi/4;     %飞行器末端期望航迹角(rad)
tDesired = 30;          %飞行器期望飞行时间25-30(s)
YnDesired = [fpaDesired; posDesired];

areaS = 0.04;           %飞行器参考面积(m^2)
airRho = 1;             %空气密度(kg/m^3)
dragC = 0.1;            %阻力系数
massM = 200;            %飞行器质量(kg)
gE = 9.8;
dragK = airRho*areaS*dragC / massM;

epsilonDis = 1;
epsilonFPA = 0.1/180*pi;
dt = 0.2;

maxK = 10;               %最大迭代次数
numN = tDesired/dt+1;
accelList = zeros(maxK+1, numN-1);
xkList = zeros(4, numN-1);

%% 画图记录量
time = 0:dt:tDesired;

%% 牛顿迭代制导
for cntk = 1:1:maxK
    % 使用当前控制序列递推后面所有状态
    xkList(:, 1) = [velocity; fpAngle; position];
    for i = 2:1:tDesired/dt+1
        xkList(:, i) = dynamicFunc(xkList(:, i-1), accelList(cntk, i-1), dt, dragK, gE);
    end

    % Fu
    Yn = xkList(2:4, end);
    Fu = Yn - YnDesired;
    if abs(Fu(1)) <= epsilonFPA && norm([Fu(2), Fu(3)]) <= epsilonDis
        break;
    end
    
    % (uk,Xk)->sk
    dFu = zeros(3, numN-1);
    pHpX = [zeros(3,1), eye(3)]; 
    for i = 1:1:numN-1
        pGpX = eye(4);
        for j = i+1:1:numN-1
            xj = xkList(:, j);
            acj = accelList(cntk, j);
            pGpXi = jacobianpGpX(xj, acj, dt, dragK, gE);
            pGpX = pGpXi * pGpX;
        end
        pGpU = [0; 1/xkList(1,i); 0; 0];
        pGpU = pGpU.*dt; %区别
        pFu = pHpX * pGpX * pGpU;
        dFu(:, i) = pFu;
    end
    sk = -pinv(dFu) * (dFu*accelList(cntk,:)' - Fu) + accelList(cntk,:)';
    
    % u(k+1)
    accelList(cntk+1, :) = accelList(cntk, :) - sk';
 
end

%% 画图
figure;
hold on;
for i = 2:1:cntk
    plot(time(1:end-1), accelList(i, :), 'LineWidth', 1.5);
end
grid on;
legend('1','2','3','4','5','6');
xlabel('time(s)');
ylabel('accel(m/s^2)');
title('牛顿迭代加速度');

figure;
plot(xkList(3,:), xkList(4,:), 'LineWidth', 1.5);
axis([0, 5000, 0, 4500]);
grid on;
xlabel('x(m)');
ylabel('y(m)');
title('牛顿法制导轨迹');

figure;
plot(time, xkList(2,:)/pi*180, 'LineWidth', 1.5);
grid on;
xlabel('time(s)');
ylabel('飞行航迹角(°)');
title('飞行航迹角变化');

%% 小函数
%动力学递推函数
function y = dynamicFunc(x, ac, dt, dragK, g)   
    velocity = x(1);
    fpAngle = x(2);
    px = x(3);
    py = x(4);
    aDrag = dragK*velocity*velocity / 2;
    y1 = velocity + (-aDrag -g*sin(fpAngle))*dt;
    y2 = fpAngle + (ac -g*cos(fpAngle))*dt/velocity;
    y3 = px + (velocity*cos(fpAngle))*dt;
    y4 = py + (velocity*sin(fpAngle))*dt;
    y = [y1; y2; y3; y4];
end

function y = jacobianpGpX(x, ac, dt, dragK, g)
    pGpX = zeros(4, 4);
    velocity = x(1);
    fpAngle = x(2);
    pGpX(1,1) = -dragK*velocity;
    pGpX(1,2) = -g*cos(fpAngle);
    pGpX(2,1) = (g*cos(fpAngle) - ac)/(velocity*velocity);
    pGpX(2,2) = g*sin(fpAngle)/velocity;
    pGpX(3,1) = cos(fpAngle);
    pGpX(3,2) = -velocity*sin(fpAngle);
    pGpX(4,1) = sin(fpAngle);
    pGpX(4,2) = velocity*cos(fpAngle);
    
    pGpX = pGpX.*dt + eye(4); %区别
    
    y = pGpX;
end
